#include "RunPoint.h"

//Track_t sTrack = {	/* 跑点结构体 */
//	.eRobotWorkState = RobotWork_Empty,
//	.eTrackState     = Track_Init,
//};

//Axis3f RunPoint_Out;		//跑点输出

//typedef struct
//{
//	PID_t xy;
//	PID_t yaw;
//}RunPointPid_t;

//RunPointPid_t Runpos_pid;
//RunPointPid_t NextRunpos_pid;

///* 运行的点 */
//Point_t RunPoint_List[10];

///* 梯形加减速形参 */
//TusrConfig_t  TusrConfig_List[10];

///* 梯形加减速数据结构 */
//Traj_Typedef Traj_xy;
//Traj_Typedef NextTraj_xy;

///**
// * @brief  跑点结束
// * @param  void
// * @retval void
// */
//void TrackPoint_End(RobotWorkState_t eRobotState)
//{
//	if ( sTrack.eTrackState != Track_End) {	/* 判断是否结束 */
//		return ;
//	}
//	
//	sRobot_Pos.x_pos = 0;
//	
//	RunPoint_Out.x = 0;
//	RunPoint_Out.y = 0;
//	RunPoint_Out.z = 0;
//	
//	PID_Reset(&Runpos_pid.yaw);
//	
//	sTrack.eTrackState = Track_Init;							/* 设置为初始化模式 */
//	sTrack.eRobotWorkState = RobotWork_LockPos; 	/* 更改跑点工作模式 */
//}

/**
 * @brief  获取曼哈顿距离
 * @param  void
 * @retval void
 */
float TrackPoint_getManhattanDis(float now_x, float end_x, /* X方向 */
																 float now_y, float end_y  /* Y方向 */)
{
	/* 计算误差 */
	float x_err = end_x - now_x; //X误差
	float y_err = end_y - now_y; //X误差

	/* 勾股定理计算距离 */
	float ManhattanDis = sqrt((x_err * x_err) + (y_err * y_err)); //曼哈顿距离
	
	return ManhattanDis;
}

///**
// * @brief  设置下一个点
// * @param  void
// * @retval void
// */
//void TrackPoint_Next(	Point_t NowPoint,					/* 开始的点 */
//											Point_t EndPoint,				  /* 结束的点 	*/
//											int num) {
//																		
//	sTrack.eTrackState 					= Track_Run;										/* 状态机切换，开始跑点 			*/

//	TusrConfig_List[num].End_pos	 = EndPoint;											/* 终点 */
//	TusrConfig_List[num].Start_pos = NowPoint;											/* 起点 */
//	
//	/* 获取曼哈顿距离 */
//	float TotalDis = TrackPoint_getManhattanDis(NowPoint.x, EndPoint.x, NowPoint.y, EndPoint.y);
//	
//	/* 获取总路程 */
//	TusrConfig_List[num].TotalDis = TotalDis;
//	TusrConfig_List[num].TotalYaw = yaw_clamp(EndPoint.z - NowPoint.z);

//	/* 初始化s型加减速 */
//	TRAJ_plan(&Traj_xy, TotalDis, TusrConfig_List[num].traj_vel,
//							 TusrConfig_List[num].vel_limit,					// Velocity
//							 TusrConfig_List[num].traj_accel,				// Acceleration
//							 TusrConfig_List[num].traj_decel);				// Deceleration
//}

///**
// * @brief  初始化跑点信息
// * @param  void
// * @retval void
// */
//void TrackPoint_Init(void) 
//{
//	if ( sTrack.eTrackState != Track_Init) {	/* 判断是否初始化 */
//		return ;
//	}
//	sTrack.NowRunPoin		= 0;	/* 从头开始跑 */
//	
//	Point_t temp;
//	
//	temp.x = sRobot_Pos.x_pos;
//	temp.y = sRobot_Pos.y_pos;
//	temp.z = sRobot_Pos.z_pos;
//	
//	TrackPoint_Next(temp, RunPoint_List[0], 0);
//}

///**
// * @brief  设置跑点参数
// * @param  void
// * @retval void
// */
//void TarckSetPos( setRunpoit_t * setPos)
//{
//	int num;
//	
//	for (num = 0; num < 32; num++) {
//		
//		/* 位置 */
//		RunPoint_List[num].x = setPos[num].x;
//		RunPoint_List[num].y = setPos[num].y;
//		RunPoint_List[num].z = setPos[num].z;
//		RunPoint_List[num].TrackNextState = setPos[num].NextState;
//		
//		/* 加减速 */
//		TusrConfig_List[num].traj_vel   = setPos[num].traj_vel;
//		TusrConfig_List[num].traj_accel = setPos[num].traj_accel;
//		TusrConfig_List[num].traj_decel = setPos[num].traj_decel;
//		TusrConfig_List[num].vel_limit  = setPos[num].vel_limit;
//		TusrConfig_List[num].useFlag    = False;
//		
//		/* 结束判断标志 */
//		if (RunPoint_List[num].TrackNextState != RobotWork_RunPos) {
//			
//			/* 记录一次锁点位置，但是不跑 */
//			num++;
//			RunPoint_List[num].x = setPos[num].x;
//			RunPoint_List[num].y = setPos[num].y;
//			RunPoint_List[num].z = setPos[num].z;
//			break;
//		}
//	}

//	/* 重置跑点状态 */
//	sTrack.eRobotWorkState = RobotWork_RunPos;
//	sTrack.eTrackState = Track_Init;
//}

///**
// * @brief  初始化跑点
// * @param  void
// * @retval void
// */
//void Tracking_init(void)
//{
//	pid_init(&Runpos_pid.yaw, -20, 0, 0, 2000, 2000);
//	pid_init(&Runpos_pid.xy, 10, 0, 0, 5000, 5000);
//	
//	pid_init(&NextRunpos_pid.yaw, -20, 0, 0, 2000, 2000);
//	pid_init(&NextRunpos_pid.xy, 10, 0, 0, 5000, 5000);	
//}

///**
// * @brief  锁点
// * @param  void
// * @retval void
// */
//void Locked_Point(Track_t *sTrack)
//{
//	RunPoint_Out.x = 0;
//	RunPoint_Out.y = 0;
//	RunPoint_Out.z = 0;
//}

///**
// * @brief  模型控制算法
// * @param  void
// * @retval void
// */
//Axis3f TrackPoint_MathOut(Traj_Typedef *Traj, TusrConfig_t TusrConfig, Point_t endPoint)
//{
//	Axis3f out;
//	
//	float x_error = endPoint.x - TusrConfig.Start_pos.x;
//	float y_error = endPoint.y - TusrConfig.Start_pos.y;
//	
//	/* 非对称S加减速 */
//	TRAJ_eval(Traj, runPoint_dt);

//	/* 计算速度 */
//	float k = TRAJ_ModelScale_ForDis(*Traj);
//	
//	/* 施加量 */
//	out.x = TusrConfig.Start_pos.x + (x_error * k);
//	out.y = TusrConfig.Start_pos.y + (y_error * k);
//	
//	/* YAW轴闭环 */
//	float ModelYaw = k * TusrConfig.TotalYaw;
//	out.z = TusrConfig.Start_pos.z + ModelYaw;

//	return out;
//}

///**
// * @brief  弧线长度 
// * @param  void
// * @retval void
// */
//float Bezier_GetLen(float t, Axis2f p0, Axis2f p1, Axis2f p2)
//{
//	float ax = p0.x - 2 * p1.x + p2.x;
//	float ay = p0.y - 2 * p1.y + p2.y;
//	float bx = 2 * p1.x - 2 * p0.x;
//	float by = 2 * p1.y - 2 * p0.y;
//	float A = 4 * (ax * ax + ay * ay);
//	float B = 4 * (ax * bx + ay * by);
//	float C = bx * bx + by * by;

//	//计算多项式的某些项的值，减少相同运算次数
//	float sa = sqrt(A); 
//	float satb = 2 * A * t + B; 
//	float satbtc= 2* sa * sqrt(A*t*t + B*t + C); 
//	float sa23 = 8 * A * sa;
//	
//	float result = (satbtc * satb - (B * B - 4 * A * C) * log(satbtc+ satb))/ sa23;
//	return result;
//}

///**
// * @brief  求出匀速的贝塞尔时间
// * @param  void
// * @retval void
// */
//float Bezier_GetTime(Axis2f *p0, Axis2f *p1, Axis2f *p2, float nowTime, float length)
//{
//	float realTime = 0.01;			// 将 rt 初始化为 0
//	float rt_length = 0;				// rt 对应的弧长初始化为 0
//	float deltaLength = 0;			// 与真实弧长的差距
//	float deltaTime = 0;				// rt 的更新量
//	float low = 0, high = 1;		// 界定二分上下限

//	length += Bezier_GetLen(nowTime, *p0, *p1, *p2);
//	
//	while(1) {
//		
//		// 半分
//		if (deltaLength > 0) {
//			// rt 对应的弧长太大了，减小 rt
//			realTime -= (float)(realTime - low) / 2;
//			
//			// rt 更新幅度
//			deltaTime = realTime - low;
//		} else {
//			// rt 对应的弧长太小了，增大 rt
//			realTime += (float)(high - realTime) / 2;
//			
//			// rt 更新幅度
//			deltaTime = high - realTime;
//		}
//		
//		// 计算弧长差值
//		rt_length = Bezier_GetLen(realTime, *p0, *p1, *p2);
//		deltaLength = rt_length - length;

//		// 更新二分上下限
//		if (deltaLength > 0){
//			high = realTime;
//		} else {
//			low = realTime;
//		}
//		/* 
//			 0.01 的误差已经很小了，可视为此时的 rt 就是真实的 rt
//			 或者 rt 的更新量足够小时也应跳出，防止梯度消失造成死循环	
//		*/
//		if ((deltaLength < 0.001f && deltaLength > -0.001f) || deltaTime < 0.00000000000000001f) {
//			// 算法收敛
//			return realTime;
//		}
//	};
//}

///**
// * @brief  开始跑点
// * @param  void
// * @retval void
// */
//void TrackPoint_Run(void)
//{
//	static Axis2f pos[3];						//贝塞尔的三个坐标 
//	static float startTime;					//贝塞尔开始的时间 
//	static Axis3f out = {0,0,0};		/* 计算跑点输出 >> 模型 */
//	
//	if ( sTrack.eTrackState != Track_Run ) {	/* 判断是否跑点 */
//		return ;
//	}
//	
//	/* 跑点完成判断 */
//	if(TRAJ_getEndFlag(Traj_xy))
//	{
//		/* 想继续跑点 */
//		if (RunPoint_List[sTrack.NowRunPoin].TrackNextState == RobotWork_RunPos) {
//			
//			sTrack.NowRunPoin++;	/* 跑点列表自加 */
//			
//			TusrConfig_List[sTrack.NowRunPoin].traj_vel = NextTraj_xy.v;
//			
//			/* 加载下一个点 */
//			TrackPoint_Next(RunPoint_List[sTrack.NowRunPoin-1], RunPoint_List[sTrack.NowRunPoin], sTrack.NowRunPoin);
//			
//			Traj_xy.t = Traj_xy.temp.accelTime;
//			buzz_didi(1);
//			return;
//		}
//		
//		/* 不想跑了 */
//		if (RunPoint_List[sTrack.NowRunPoin].TrackNextState == RobotWork_LockPos) {
//			sTrack.eTrackState = Track_End;
//			sTrack.eRobotWorkState = RobotWork_LockPos;
//			buzz_didi(2);
//			return;
//		}
//		
//		/* 直接当个摆烂仔，底盘都不控了 */
//		if (RunPoint_List[sTrack.NowRunPoin].TrackNextState == RobotWork_Empty) {
//			sTrack.eTrackState = Track_End;
//			sTrack.eRobotWorkState = RobotWork_Empty;
//			buzz_didi(3);
//			return;
//		}
//	}

//	Axis3f ModelOut;
//	
//	/*
//		当加减速进入减速环节时
//		判断下一个点是否需要跑，需要的话就提前跑这个点
//	*/
//		if ( TRAJ_getStep(Traj_xy) >= 4 && RunPoint_List[sTrack.NowRunPoin].TrackNextState == RobotWork_RunPos ) {
//		
//		uint8_t idx = sTrack.NowRunPoin+1;
//		
//		/* 判断点是否被使用过，没有使用过就初始化它 */
//		if (TusrConfig_List[idx].useFlag == False) {
//			/* 
//				手动初始化
//			使用当前正在减速的末位置作为下一次的初始位置
//			注意，这次我们将数据保存在了跑点列表里面
//			*/
//			TusrConfig_List[idx].traj_vel = 0;
//			TusrConfig_List[idx].TotalDis = TrackPoint_getManhattanDis(RunPoint_List[idx-1].x, RunPoint_List[idx].x, RunPoint_List[idx-1].y, RunPoint_List[idx].y);
//			TusrConfig_List[idx].TotalYaw = yaw_clamp(RunPoint_List[idx].z - RunPoint_List[idx-1].z);
//			TusrConfig_List[idx].Start_pos.x   = RunPoint_List[idx-1].x;
//			TusrConfig_List[idx].Start_pos.y   = RunPoint_List[idx-1].y;
//			TusrConfig_List[idx].Start_pos.z   = RunPoint_List[idx-1].z;

//			
//			TRAJ_plan(&NextTraj_xy, TusrConfig_List[idx].TotalDis, 0,
//								 TusrConfig_List[idx].vel_limit,				// Velocity
//								 TusrConfig_List[idx].traj_accel,				// Acceleration
//								 TusrConfig_List[idx].traj_decel);				// Deceleration
//			/* 告诉别人已经使用过了 */
//			TusrConfig_List[idx].useFlag = True;
//			
//			/* 贝塞尔匀速初始化 */
//			pos[0].x = out.x;
//			pos[0].y = out.y;
//			
//			pos[1].x = RunPoint_List[idx-1].x;
//			pos[1].y = RunPoint_List[idx-1].y;
//			
//			float k = NextTraj_xy.temp.s2 / NextTraj_xy.temp.EndPoint;

//			pos[2].x = RunPoint_List[idx-1].x + ((RunPoint_List[idx].x - RunPoint_List[idx-1].x) * k);
//			pos[2].y = RunPoint_List[idx-1].y + ((RunPoint_List[idx].y - RunPoint_List[idx-1].y) * k);
//			
//			startTime = Traj_xy.t;
//		}
//		/* 匀速状态的位置 */
//		Axis2f BezierPos[3];
//		
//		/* 贝塞尔曲线 */
//		if (Traj_xy.t < startTime + 1) {

//			Traj_xy.t = startTime + Bezier_GetTime(&pos[0], &pos[1], &pos[2], Traj_xy.t - startTime, Traj_xy.temp.v2 * runPoint_dt);
//			
//			float t = Traj_xy.t - startTime;
//			
//			/* 贝塞尔路径规划 */
//			/* 一阶 */
//			BezierPos[0].x = pos[0].x + ((pos[1].x - pos[0].x) * t);
//			BezierPos[0].y = pos[0].y + ((pos[1].y - pos[0].y) * t);
//			
//			BezierPos[1].x = pos[1].x + ((pos[2].x - pos[1].x) * t);
//			BezierPos[1].y = pos[1].y + ((pos[2].y - pos[1].y) * t);
//			
//			/* 二阶 */
//			BezierPos[2].x = BezierPos[0].x + ((BezierPos[1].x - BezierPos[0].x) * t);
//			BezierPos[2].y = BezierPos[0].y + ((BezierPos[1].y - BezierPos[0].y) * t);
//			
//			out.x = BezierPos[2].x;
//			out.y = BezierPos[2].y;

//		} else {
//			Traj_xy.step = 6;
//			return;
//		}
//	} else {
//		ModelOut = TrackPoint_MathOut(&Traj_xy,  TusrConfig_List[sTrack.NowRunPoin], TusrConfig_List[sTrack.NowRunPoin].End_pos);
//		out.x = ModelOut.x;
//		out.y = ModelOut.y;
//		out.z = ModelOut.z;
//	}

//	/* 
//		输出接口赋值
//		模型与现实用pid跟踪 
//		注意，pid没有使用积分微分，只有p作用，所以共用一套pid
//	*/
////	RunPoint_Out.x = pid_calculate_position( &Runpos_pid.xy, out.x, sRobot_Pos.x_pos, 0.001f );
////	RunPoint_Out.y = pid_calculate_position( &Runpos_pid.xy, out.y, sRobot_Pos.y_pos, 0.001f );
////	RunPoint_Out.z = yaw_pid_updata( &Runpos_pid.yaw, out.z, sRobot_Pos.z_pos, 0.001f );
//}

///**
// * @brief  跑点
// * @param  void
// * @retval void
// */
//void Tracking_Task(void)  //跑点
//{	
//	/*
//		跑点逻辑
//		根据状态机运行正确的跑点逻辑，TrackRunTask为指针函数，指向用户自己写的跑点逻辑
//	*/
//	if ( sTrack.eRobotWorkState == RobotWork_Empty ) {		// >>空状态，不进行任何底盘控制
//		
//		/* 
//			... 
//		*/
//		
//	} else if ( sTrack.eRobotWorkState == RobotWork_LockPos ) {	// >>锁点状态
//		
//		Locked_Point(&sTrack);	/* 锁点 */
//		
//	} else if ( sTrack.eRobotWorkState == RobotWork_RunPos ) {
//		
//		/* 跑点初始化 */
//		TrackPoint_Init();
//		
//		/* 开始跑点 */
//		TrackPoint_Run();
//		
//		/* 跑点完成 */
//		TrackPoint_End(RobotWork_LockPos);
//	} else {
//		/* ... */
//	}
//}
